// 测试库：SimpleFOC 2.2.1
//测试硬件：SimpleFOC_X V1.0驱动板
//淘宝店铺：YOUNGFOC    B站：LazierWoo     闲鱼店铺：YOUNGFOC  立创开源广场 用户名：LazyWoo
//程序功能：速度闭环控制，使用电流环闭环。在串口窗口中输入：T+目标速度，就可以使得电机闭环达到指定转速。
//比如让电机转速为50 rad/s，则在串口输入：T50.0
//注意事项：速度闭环控制中，输入的目标速度单位是弧度每秒，6.28rad/s表示每秒一转，也就是RPM=60；

//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
//☆☆☆☆☆☆在程序下，请保证输入的目标速度在-80 至 80 rad/s之间，☆☆☆☆☆☆☆☆☆☆
//☆☆若单次设定速度变化过大，电机会失控并提升到一个很高的转速，请立即复位电机☆☆☆☆
//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
//☆☆☆☆☆☆程序默认设置的供电电压为 12V,用其他电压供电请记得修改!!!!!☆☆☆☆☆☆☆☆☆
//☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆


#include <SimpleFOC.h>
#include <Arduino.h> // 包含 FreeRTOS 库

//创建电机对象
BLDCMotor motor = BLDCMotor(7); //极对数
//创建驱动器对象
//3PWM模式，定义三个PWM输出口，以及使能口
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 32, 33,26);


//创建AS5600磁编码器对象
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);

//创建内置电流传感器，两个ADC引脚对应两相的电流采样
//采样电阻10mΩ，0.01欧姆，INA240A2放大倍数50.
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 34, 35);

/*
 * 
 * BLDCDriver3PWM driver = BLDCDriver3PWM(25, 32, 33,26);
 * InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 34, 35);
 * 这两句中，25与34，32与35是相互对应的电机相，顺序不要修改
*/


//命令设置
float target_velocity = 0;
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }

// 定义任务句柄
TaskHandle_t torquePrintTaskHandle;

// Torque 打印任务
void torquePrintTask(void *pvParameters) {
  // const TickType_t xDelay = pdMS_TO_TICKS(100); // 每 100 毫秒运行一次
  // float Kt = 0.036322227;
  // float Torque = 0.0;

  for (;;) {
    // 获取当前角速度（rad/s）
    float current_velocity = motor.shaftVelocity();
    
    // 打印 Torque 值和当前角速度
    Serial.print(motor.current.q);  // 打印 q 轴电流值
    Serial.print(" ");              // 添加空格分隔
    Serial.println(current_velocity); // 打印当前角速度
    // Torque = Kt * motor.current.q;
    // Serial.println(motor.current.q); // 打印 Torque 值
    vTaskDelay(5); // 延迟 5 毫秒
  }
}

void setup() 
{
  //磁编码器初始化
  I2Cone.begin(19,18, 400000UL);
  sensor.init(&I2Cone);
  //关联电机对象与磁编码器对象
  motor.linkSensor(&sensor);

  // 驱动器参数
  // ☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆供电电压☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
  driver.voltage_power_supply = 24.0;  //板子上带有防反接二极管，在使用过程中约有0.3V的压降，可以减去0.3，也可以忽略
  //驱动器初始化
  driver.init();
  //关联电机对象和驱动器对象
  motor.linkDriver(&driver);

  // 电流传感器初始化
  current_sense.init();
  //关联电机对象和电流传感器对象
  motor.linkCurrentSense(&current_sense);
  
  //力矩控制模式选择：FOC电流（foc_current）、DC电流（dc_current）、电压（voltage）
  motor.torque_controller = TorqueControlType::foc_current; 
  // 运动控制模式选择：力矩（torque）、速度(velocity)、位置(angle)
  motor.controller = MotionControlType::velocity;

  //FOC电流环参数，只有在电流闭环时才有用
  //dc_current模式下只有Current q项PID
  //foc_current模式下Current q项PID、Current d项PID都起作用

  //Current q项参数
   motor.PID_current_q.P = 3.0;
   motor.PID_current_q.I = 300.0;
   motor.PID_current_q.D = 0.0;
   motor.PID_current_q.output_ramp = 500.0;
   //motor.PID_current_q.limit = 8.0;  //电流环的输出限制，应该等于限制电压motor.voltage_limit；
   //低通滤波器 
   motor.LPF_current_q.Tf = 0.005;

  //Current d项参数
  motor.PID_current_d.P = 3.0;
  motor.PID_current_d.I = 300.0;
  motor.PID_current_d.D = 0.0;
  motor.PID_current_d.output_ramp = 500.0;
  //motor.PID_current_d.limit = 8.0;  //电流环的输出限制，应该等于限制电压motor.voltage_limit；
  //低通滤波器 
  motor.LPF_current_d.Tf = 0.005;

   
  // 速度环PID参数
  motor.PID_velocity.P = 0.12;
  motor.PID_velocity.I = 1.2;
  motor.PID_velocity.D = 0.001;
  motor.PID_velocity.output_ramp = 1000.0;
  //速度环的输出限制:在电流闭环时，此限制应该等于限制电流motor.current_limit；
  //速度环的输出限制:在使用电压力矩时（电流开环），此限制应该等于限制电压motor.voltage_limit； 
  //motor.PID_velocity.limit = 0.8; 
  // 低通滤波器
  motor.LPF_velocity.Tf = 0.02;
  
  // // 角度环PID参数
  // motor.P_angle.P = 10.0;
  // motor.P_angle.I = 0.0;
  // motor.P_angle.D = 0.0;
  // motor.P_angle.output_ramp = 1000.0;
  // //motor.P_angle.limit = 30.0;    //角度环的输出限制就等于速度限制velocity_limit
  // // 低通滤波器
  // motor.LPF_angle.Tf = 0.01;

  
  // 设置限制
  motor.velocity_limit = 30.0; //速度限制，指角度模式下的速度限制
  //把速度限制设小，比如设置成3.0或者5.0，角度控制模式下，最高速度会变小，
  motor.voltage_limit = 8.0;  //电机限制电压
  motor.current_limit = 0.8;  //电机限制电流


  //串口用于接受指令
  Serial.begin(115200);
  motor.useMonitoring(Serial);

  // 电机初始化
  motor.init();
  // 初始化FOC
  motor.initFOC();

  //使用字母T作为命令前缀
  command.add('V', doTarget, "target_velocity");

  //初始化成功，等待接收命令
  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target velocity using serial terminal:"));
  _delay(1000);

  // 创建 Torque 打印任务
  xTaskCreate(
    torquePrintTask,          // 任务函数
    "TorquePrintTask",        // 任务名称
    1024,                      // 任务堆栈大小
    NULL,                     // 任务参数
    1,                        // 任务优先级
    &torquePrintTaskHandle    // 任务句柄
  );

  // 启动调度器
  vTaskStartScheduler();
}

void loop() 
{
  // 主任务代码
  motor.loopFOC();

  // 从串口读取目标速度
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n'); // 读取一行输入
    input.trim(); // 去除前后空格
    if (input.startsWith("V")) { // 检查是否以 'V' 开头
      float new_velocity = input.substring(1).toFloat(); // 提取速度值并转换为浮点数
      if (new_velocity >= -80.0 && new_velocity <= 80.0) { // 检查速度范围
        target_velocity = new_velocity;
        Serial.print(F("Target velocity set to: "));
        Serial.println(target_velocity);

        // // 添加 5 秒延迟
        // Serial.println(F("Motor will start moving in 5 seconds..."));
        // delay(5000); // 延迟 5 秒
      } else {
        Serial.println(F("Error: Target velocity out of range (-80 to 80)."));
      }
    } else {
      Serial.println(F("Error: Invalid command. Use 'V<value>' to set target velocity."));
    }
  }

  // 根据目标速度移动电机
  motor.move(target_velocity);

  // 运行命令解析器
  command.run();
}
